﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Kinect;
using Imaging;
using System.Drawing;
using KinectApplication;
using Microsoft.Samples.Kinect.WpfViewers;
using KinectDwa.Parsers.Helpers;
using KinectDwa.InputDevices;
using System.Windows.Media;
using System.IO;
using System.Windows.Controls;
using System.Windows.Input;
using KinectDwa.Parsers.Data;

namespace KinectDwa.Parsers
{
    public class SkeletonMouse
    {
        // Some conditions for potential gesture before bothering to check it
        private const int MinGestureWidth = 20;
        private const int MinGestureHeight = 30;

        // Current user data, don´t touch
        public static int playerID = -1;
        public static bool rightHand = true;
        private static Dictionary<int, Player> oldPlayers = new Dictionary<int, Player>();

        // Current mouse tracking data, don´t touch
        private static double skeletonCordX = 0;
        private static double skeletonCordY = 0;
        private static double mouseCordX = 0;
        private static double mouseCordY = 0;

        // Debugging helpers
        private static bool _debugPending;

        #region Mouse icon handling

        // List of objects that get the new cursor effects
        private static List<UserControl> iconControls = new List<UserControl>();

        // Special kinect mousing cursors
        private static Cursor waitCursor;
        private static Cursor MoveCursor;
        private static Cursor leftCursor;
        private static Cursor RightCursor;

        // Add user control to have the mouse drawn on it
        public static void AddUserControl(UserControl iconControl)
        {
            if (waitCursor == null)
            {
                waitCursor = new Cursor(Environment.CurrentDirectory + "\\Cursors\\waiting.cur");
                MoveCursor = new Cursor(Environment.CurrentDirectory + "\\Cursors\\moving.cur");
                leftCursor = new Cursor(Environment.CurrentDirectory + "\\Cursors\\leftclick.cur");
                RightCursor = new Cursor(Environment.CurrentDirectory + "\\Cursors\\rightclick.cur");
            }

            iconControl.Cursor = waitCursor;
            iconControls.Add(iconControl);
        }

        // Remove user control from mousing icons list
        public static void RemoveUserControl(UserControl iconControl)
        {
            iconControls.Remove(iconControl);
        }

        private static void SetCursor(Cursor cursor)
        {
            foreach (UserControl control in iconControls)
            {
                control.Cursor = cursor;
            }
        }

        #endregion

        public static void Parse(KinectSensor sender, AllFramesReadyEventArgs e)
        {
            Debugging.Clear();

            _debugPending = Debugging.ShowDepthData; 
            List<Player> playerData = new List<Player>();

            // Gather up the skeletons
            using (SkeletonFrame skeletonFrame = e.OpenSkeletonFrame())
            {
                if (skeletonFrame == null)
                    return;

                Skeleton[] skeletonCollection = new Skeleton[skeletonFrame.SkeletonArrayLength];
                skeletonFrame.CopySkeletonDataTo(skeletonCollection);

                // Create a list of skeletons, adding gesture data on the tracked ones
                foreach (Skeleton skeleton in skeletonCollection)
                {
                    if (skeleton.TrackingState != SkeletonTrackingState.NotTracked)
                    {
                        Player newPlayer;

                        try
                        {
                            newPlayer = oldPlayers[skeleton.TrackingId];
                        }
                        catch (KeyNotFoundException)
                        {
                            newPlayer = new Player();
                            newPlayer.Id = skeleton.TrackingId;
                        }

                        newPlayer.IsTracked = (skeleton.TrackingState == SkeletonTrackingState.Tracked);

                        if (skeleton.Joints[JointType.HandLeft].TrackingState == JointTrackingState.Tracked)
                        {
                            newPlayer.LeftHand.vector = skeleton.Joints[JointType.HandLeft].Position;
                            newPlayer.LeftHand.isTracked = true;
                        }
                        else
                        {
                            newPlayer.LeftHand.isTracked = false;
                        }
                        
                        if (skeleton.Joints[JointType.WristLeft].TrackingState == JointTrackingState.Tracked)
                        {
                            newPlayer.LeftHand.wristVector = skeleton.Joints[JointType.WristLeft].Position;
                            newPlayer.LeftHand.wristTracked = true;
                        }

                        if (skeleton.Joints[JointType.HandRight].TrackingState == JointTrackingState.Tracked)
                        {
                            newPlayer.RightHand.vector = skeleton.Joints[JointType.HandRight].Position;
                            newPlayer.RightHand.isTracked = true;
                        }
                        else
                        {
                            newPlayer.RightHand.isTracked = false;
                        }

                        if (skeleton.Joints[JointType.WristRight].TrackingState == JointTrackingState.Tracked)
                        {
                            newPlayer.RightHand.wristVector = skeleton.Joints[JointType.WristRight].Position;
                            newPlayer.RightHand.wristTracked = true;
                        }

                        if (skeleton.Joints[JointType.ShoulderLeft].TrackingState == JointTrackingState.Tracked)
                        {
                            if (newPlayer.Id == playerID)
                            {
                                newPlayer.LeftShoulder.ShoulderDepthCollection.Add(skeleton.Joints[JointType.ShoulderLeft].Position);

                                int collectionCount = newPlayer.LeftShoulder.ShoulderDepthCollection.Count();

                                if (collectionCount > MouseInputControl.shoulderMemoryFrames)
                                {
                                    newPlayer.LeftShoulder.ShoulderDepthCollection.RemoveAt(0);
                                }

                                float shoulderAverageZ = 0f;
                                float shoulderAverageX = 0f;
                                float shoulderAverageY = 0f;

                                foreach (SkeletonPoint shoulderPoint in newPlayer.LeftShoulder.ShoulderDepthCollection)
                                {
                                    shoulderAverageZ += shoulderPoint.Z;
                                    shoulderAverageX += shoulderPoint.X;
                                    shoulderAverageY += shoulderPoint.Y;
                                }

                                shoulderAverageZ /= collectionCount;
                                shoulderAverageX /= collectionCount;
                                shoulderAverageY /= collectionCount;

                                newPlayer.LeftShoulder.vector.Z = shoulderAverageZ;
                                newPlayer.LeftShoulder.vector.X = shoulderAverageX;
                                newPlayer.LeftShoulder.vector.Y = shoulderAverageY;
                            }
                            else
                            {
                                newPlayer.LeftShoulder.vector = skeleton.Joints[JointType.ShoulderLeft].Position;
                                newPlayer.LeftShoulder.isTracked = true;

                                if (newPlayer.LeftShoulder.ShoulderDepthCollection.Count() > 0)
                                {
                                    newPlayer.LeftShoulder.ShoulderDepthCollection.Clear();
                                }
                            }
                        }

                        if (skeleton.Joints[JointType.ShoulderRight].TrackingState == JointTrackingState.Tracked)
                        {
                            if (newPlayer.Id == playerID)
                            {
                                newPlayer.RightShoulder.ShoulderDepthCollection.Add(skeleton.Joints[JointType.ShoulderRight].Position);

                                int collectionCount = newPlayer.RightShoulder.ShoulderDepthCollection.Count();

                                if (collectionCount > MouseInputControl.shoulderMemoryFrames)
                                {
                                    newPlayer.RightShoulder.ShoulderDepthCollection.RemoveAt(0);
                                }

                                float shoulderAverageZ = 0f;
                                float shoulderAverageX = 0f;
                                float shoulderAverageY = 0f;

                                foreach (SkeletonPoint shoulderPoint in newPlayer.RightShoulder.ShoulderDepthCollection)
                                {
                                    shoulderAverageZ += shoulderPoint.Z;
                                    shoulderAverageX += shoulderPoint.X;
                                    shoulderAverageY += shoulderPoint.Y;
                                }

                                shoulderAverageZ /= collectionCount;
                                shoulderAverageX /= collectionCount;
                                shoulderAverageY /= collectionCount;

                                newPlayer.RightShoulder.vector.Z = shoulderAverageZ;
                                newPlayer.RightShoulder.vector.X = shoulderAverageX;
                                newPlayer.RightShoulder.vector.Y = shoulderAverageY;
                            }
                            else
                            {
                                newPlayer.RightShoulder.vector = skeleton.Joints[JointType.ShoulderRight].Position;
                                newPlayer.RightShoulder.isTracked = true;

                                if (newPlayer.RightShoulder.ShoulderDepthCollection.Count() > 0)
                                {
                                    newPlayer.RightShoulder.ShoulderDepthCollection.Clear();
                                }

                            }
                        }

                        playerData.Add(newPlayer);
                    }
                }
            }

            // No previous player, see if we have new volunteers
            if (playerID == -1)
            {
                foreach (Player player in playerData)
                {
                    if (player.IsTracked)
                    {
                        if ((player.RightHand.vector.Z < (player.RightShoulder.vector.Z - MouseInputControl.gainMouseControlTreshold)) && (MouseInputControl.handRight != false))
                        {
                            if ((!MouseInputControl.mouseDragging) || (player.RightHand.Gesture != HandGestures.None))
                            {
                                playerID = player.Id;
                                // DebugMessage = "Player " + playerID + " started mousing. (Right hand)";
                                rightHand = true;

                                // Starting location for hand when doing mousetracking
                                skeletonCordX = player.RightHand.vector.X * MouseInputControl.draggingMouseSensitivity + MouseInputControl.mouseCenterX;
                                skeletonCordY = (player.RightHand.vector.Y * -1) * MouseInputControl.draggingMouseSensitivity + MouseInputControl.mouseCenterY;
                            }

                        }
                        else if ((player.LeftHand.vector.Z < (player.LeftShoulder.vector.Z - MouseInputControl.gainMouseControlTreshold)) && (MouseInputControl.handRight != true))
                        {
                            if ((!MouseInputControl.mouseDragging) || (player.LeftHand.Gesture != HandGestures.None))
                            {
                                playerID = player.Id;
                                // DebugMessage = "Player " + playerID + " started mousing. (Left hand)";
                                rightHand = false;

                                // Starting location for hand when doing mousetracking
                                skeletonCordX = player.LeftHand.vector.X * MouseInputControl.draggingMouseSensitivity + MouseInputControl.mouseCenterX;
                                skeletonCordY = (player.LeftHand.vector.Y * -1) * MouseInputControl.draggingMouseSensitivity + MouseInputControl.mouseCenterY;
                            }
                        }
                    }
                }
            }
            
            // Sort out gestures from depth data
            using (DepthImageFrame depthFrame = e.OpenDepthImageFrame())
            {
                if (depthFrame != null)
                {
                    int arrayLength = depthFrame.PixelDataLength;
                    short[] depthArray = new short[arrayLength];
                    depthFrame.CopyPixelDataTo(depthArray);

                    foreach (Player player in playerData)
                    {
                        if ((player.IsTracked) && (playerID == player.Id))
                        {
                            if ((player.RightHand.isTracked) && (rightHand))
                            {
                                if (MouseInputControl.dualWield)
                                {
                                    if (player.LeftHand.isTracked)
                                    {
                                        GetGesture(sender, player.LeftHand, player.LeftShoulder, depthArray, false, player.Id);
                                        player.RightHand.Gesture = player.LeftHand.Gesture;
                                        player.RightHand.GestureAccuracy = player.LeftHand.GestureAccuracy;
                                    }
                                    else
                                    {
                                        player.RightHand.Gesture = HandGestures.None;
                                    }
                                }
                                else
                                {
                                    GetGesture(sender, player.RightHand, player.RightShoulder, depthArray, true, player.Id);
                                }
                            }
                            else if ((player.LeftHand.isTracked) && (!rightHand))
                            {
                                if (MouseInputControl.dualWield)
                                {
                                    if (player.RightHand.isTracked)
                                    {
                                        GetGesture(sender, player.RightHand, player.RightShoulder, depthArray, true, player.Id);
                                        player.LeftHand.Gesture = player.RightHand.Gesture;
                                        player.LeftHand.GestureAccuracy = player.RightHand.GestureAccuracy;
                                    }
                                    else
                                    {
                                        player.LeftHand.Gesture = HandGestures.None;
                                    }
                                }
                                else
                                {
                                    GetGesture(sender, player.LeftHand, player.LeftShoulder, depthArray, false, player.Id);
                                }
                            }
                        }
                    }

                    if (_debugPending)
                    {
                        _debugPending = false;

                        KinectDepthViewer.ShowDepth(depthArray, 640, 480, false, 0);

                        if (pushDebugPoint != null)
                        {
                            if (pushDebugCount >= 60)
                            {
                                pushDebugPoint = null;
                            }
                            else
                            {
                                ++pushDebugCount;
                                Debugging.ShowXY(((DepthImagePoint)pushDebugPoint).X, ((DepthImagePoint)pushDebugPoint).Y);
                            }
                        }
                    }
                }
            }

            // Get rid of old players
            oldPlayers.Clear();

            // Add new player data (and possibly old players that survived the ordeal
            foreach (Player newPlayer in playerData)
            {
                oldPlayers.Add(newPlayer.Id, newPlayer);
            }

            // Emulate the mouse
            if (playerID != -1)
            {
                DoMouseInput(playerData);
            }
        }

        // Debugging push data
        private static int pushDebugCount = 0;
        private static DepthImagePoint? pushDebugPoint = null;
        //private static bool gestureRecording = true;
        private static void GetGesture(KinectSensor sender, Hand hand, KinectDwa.Parsers.Data.Joint shoulder, short[] depthArray, bool isRight, int playerId)
        {
            if (!hand.isTracked)
            {
                hand.Gesture = HandGestures.None;
                return;
            }

            // Display debug data before its handled by gesture code
            if ((!MouseInputControl.pushGestures) && (_debugPending))
            {
                KinectDepthViewer.ShowDepth(depthArray, 640, 480, false, 0);
            }

            DepthImagePoint handDepthPoint = sender.MapSkeletonPointToDepth(hand.vector, DepthImageFormat.Resolution640x480Fps30);

            Debugging.ShowPoint(handDepthPoint.X, handDepthPoint.Y, Colors.Red);

            if (MouseInputControl.pushGestures)
            {
                DepthImagePoint shoulderDepthPoint = sender.MapSkeletonPointToDepth(shoulder.vector, DepthImageFormat.Resolution640x480Fps30);


                if (MouseInputControl.dualWield == true)
                {
                    if (hand.previousPushDepth != null)
                    {
                        hand.pushDecay += (shoulderDepthPoint.Depth - handDepthPoint.Depth) - (int)hand.previousPushDepth - MouseInputControl.decaySpeed;
                    }
                }
                else
                {
                    if (hand.previousPushDepth != null)
                    {
                        hand.pushDecay += (shoulderDepthPoint.Depth - handDepthPoint.Depth) - (int)hand.previousPushDepth - MouseInputControl.OneHandedDecaySpeed;
                    }
                }

                hand.previousPushDepth = shoulderDepthPoint.Depth - handDepthPoint.Depth;

                if (hand.pushDecay <= 0)
                {
                    hand.pushDecay = 0;
                    hand.pushClickPending = true;
                }

                if (MouseInputControl.dualWield == true)
                {

                    if ((hand.pushDecay >= MouseInputControl.pushTreshold) && (hand.pushClickPending))
                    {
                        hand.pushClickPending = false;
                        hand.previousPushDepth = null;

                        hand.Gesture = HandGestures.A;
                        hand.GestureAccuracy = 1.0D;
                    }
                    else
                    {
                        hand.Gesture = HandGestures.None;
                    }
                }
                //one handed push is now more forgiving
                else
                {
                    if ((hand.pushDecay >= MouseInputControl.OneHandedPushTreshold) && (hand.pushClickPending))
                    {
                        hand.pushClickPending = false;
                        hand.previousPushDepth = null;

                        hand.Gesture = HandGestures.A;
                        hand.GestureAccuracy = 1.0D;
                    }
                    else
                    {
                        hand.Gesture = HandGestures.None;
                    }
                }

                Debugging.ShowText(hand.pushDecay.ToString());

                // Debug square time
                if ((hand.Gesture == HandGestures.A) && (MouseInputControl.requiredGestureAccuracy <= hand.GestureAccuracy))
                {
                    pushDebugCount = 0;
                    pushDebugPoint = handDepthPoint;
                }
            }
            else // Hand gestures
            {
                // New and buggy way to get hand-info from depth array
                HandInfo info = null;

                info = depthArray.GetHandInfo(handDepthPoint.X, handDepthPoint.Y, 640, 480, hand.GetAngleAsDegrees());

                if (info == null)
                {
                    hand.Gesture = HandGestures.None;
                    return;
                }

                Debugging.ShowHandImages(info);

                if ((MinGestureHeight <= info.Height) && (MinGestureWidth <= info.Width))
                {
                    HandGestureReader.IdentifyGesture(isRight, info.ResizedArray, hand);
                    
                    /*
                    // Records hand gesture in a witty inventive way
                    if (gestureRecording)
                    {
                        gestureRecording = false;
                        HandGestureReader.SetLearnGesture(HandGestures.None);
                    }
                    HandGestureReader.LearnGestureEvent(true, info.ResizedArray);
                    */

                    //HandGestureReader.GestureSwitcheroo();

                    // Display Debug Data instead of the default one
                    if (_debugPending)
                    {
                        _debugPending = false;
                        // Moved to before GetHandInfo
                        //KinectDepthViewer.ShowDepth(depthArray, 640, 480, true, info.MinDepth);
                        int degrees = (int)(90 - hand.GetAngleAsDegrees());
                        Debugging.ShowText("Hand angle " + degrees.ToString() + " degrees!");

                        if ((hand.Gesture == HandGestures.Neutral) && (MouseInputControl.requiredGestureAccuracy <= hand.GestureAccuracy))
                        {
                            Debugging.ShowRectangle(info.Left, info.Top, info.Width, info.Height, Colors.Blue);
                        }
                        else if ((hand.Gesture == HandGestures.A) && (MouseInputControl.requiredGestureAccuracy <= hand.GestureAccuracy))
                        {
                            Debugging.ShowRectangle(info.Left, info.Top, info.Width, info.Height, Colors.Red);
                        }
                        else if ((hand.Gesture == HandGestures.B) && (MouseInputControl.requiredGestureAccuracy <= hand.GestureAccuracy))
                        {
                            Debugging.ShowRectangle(info.Left, info.Top, info.Width, info.Height, Colors.Yellow);
                        }
                        else
                        {
                            Debugging.ShowRectangle(info.Left, info.Top, info.Width, info.Height, Colors.Green);
                        }
                    }
                }
            }
        }

        private static void DoMouseInput(List<Player> players)
        {
            // try to get input from old player, see if player has gone missing or quits mousing
            bool playerLost = true;

            foreach (Player player in players)
            {
                if (playerID == player.Id)
                {
                    if (player.IsTracked)
                    {
                        if (((rightHand) && (player.RightShoulder.vector.Z < (player.RightHand.vector.Z + MouseInputControl.loseMouseControlTreshold)))
                            || ((!rightHand) && (player.LeftShoulder.vector.Z < (player.LeftHand.vector.Z + MouseInputControl.loseMouseControlTreshold))))
                        {
                            // DebugMessage = "Player " + playerID + " quit mousing.";
                            playerID = -1;
                            playerLost = false;
                        }
                        else
                        {
                            playerLost = false;

                            float handX;
                            float handY;
                            float shoulderX;
                            float shoulderY;
                            bool leftClick;
                            bool rightClick;
                            bool mouseDrag;

                            if (rightHand)
                            {
                                handX = player.RightHand.vector.X;
                                handY = player.RightHand.vector.Y * -1f;

                                if (player.RightShoulder.isTracked)
                                {
                                    // TODO: modify center point here! us values added to MouseInputControl
                                    shoulderX = player.RightShoulder.vector.X + MouseInputControl.shoulderAdjustX;
                                    shoulderY = (player.RightShoulder.vector.Y * -1f) + MouseInputControl.shoulderAdjustY;
                                }
                                else
                                {
                                    shoulderX = 0f;
                                    shoulderY = 0f;
                                }

                                leftClick = ((player.RightHand.GestureAccuracy >= MouseInputControl.requiredGestureAccuracy) && (player.RightHand.Gesture == HandGestures.A));
                                rightClick = ((player.RightHand.GestureAccuracy >= MouseInputControl.requiredGestureAccuracy) && (player.RightHand.Gesture == HandGestures.B));
                                mouseDrag = ((player.RightHand.GestureAccuracy >= MouseInputControl.requiredGestureAccuracy) && (player.RightHand.Gesture == HandGestures.Neutral));
                            }
                            else
                            {
                                handX = player.LeftHand.vector.X;
                                handY = player.LeftHand.vector.Y * -1f;

                                if (player.LeftShoulder.isTracked)
                                {
                                    // TODO: modify center point here! use negative version of values added to MouseInputControl
                                    shoulderX = player.LeftShoulder.vector.X - MouseInputControl.shoulderAdjustX;
                                    shoulderY = (player.LeftShoulder.vector.Y * -1f) - MouseInputControl.shoulderAdjustY;
                                }
                                else
                                {
                                    shoulderX = 0f;
                                    shoulderY = 0f;
                                }

                                leftClick = ((player.LeftHand.GestureAccuracy >= MouseInputControl.requiredGestureAccuracy) && (player.LeftHand.Gesture == HandGestures.A));
                                rightClick = ((player.LeftHand.GestureAccuracy >= MouseInputControl.requiredGestureAccuracy) && (player.LeftHand.Gesture == HandGestures.B));
                                mouseDrag = ((player.LeftHand.GestureAccuracy >= MouseInputControl.requiredGestureAccuracy) && (player.LeftHand.Gesture == HandGestures.Neutral));
                            }

                            if (MouseInputControl.mouseDragging)
                            {
                                if (leftClick || rightClick || mouseDrag)
                                {
                                    if (leftClick)
                                    {
                                        SetCursor(leftCursor);
                                    }
                                    else if (rightClick)
                                    {
                                        SetCursor(RightCursor);
                                    }
                                    else
                                    {
                                        SetCursor(MoveCursor);
                                    }

                                    double newLocationX = handX * MouseInputControl.draggingMouseSensitivity + MouseInputControl.mouseCenterX;
                                    double newLocationY = handY * MouseInputControl.draggingMouseSensitivity + MouseInputControl.mouseCenterY;
                                    double changeX = newLocationX - skeletonCordX;
                                    double changeY = newLocationY - skeletonCordY;
                                    skeletonCordX = newLocationX;
                                    skeletonCordY = newLocationY;
                                    mouseCordX += changeX;
                                    mouseCordY += changeY;

                                    if (mouseCordX < 0D)
                                    {
                                        mouseCordX = 0D;
                                    }
                                    else if (mouseCordX > System.Windows.SystemParameters.PrimaryScreenWidth)
                                    {
                                        mouseCordX = System.Windows.SystemParameters.PrimaryScreenWidth;
                                    }

                                    if (mouseCordY < 0D)
                                    {
                                        mouseCordY = 0D;
                                    }
                                    else if (mouseCordY > System.Windows.SystemParameters.PrimaryScreenHeight)
                                    {
                                        mouseCordY = System.Windows.SystemParameters.PrimaryScreenHeight;
                                    }

                                    MouseEmulator.MouseEvent((int)mouseCordX, (int)mouseCordY, leftClick, rightClick);
                                }
                                else
                                {
                                    // DebugMessage = "Player " + playerID + " no longer gesturing!";
                                    SetCursor(waitCursor);
                                    //playerID = -1;
                                }
                            }
                            else
                            {
                                if (leftClick)
                                {
                                    SetCursor(leftCursor);
                                }
                                else if (rightClick)
                                {
                                    SetCursor(RightCursor);
                                }
                                else
                                {
                                    SetCursor(MoveCursor);
                                }

                                MouseEmulator.MouseEvent((int)((handX - shoulderX) * MouseInputControl.normalMouseSensitivity + MouseInputControl.mouseCenterX), (int)((handY - shoulderY) * MouseInputControl.normalMouseSensitivity + MouseInputControl.mouseCenterY), leftClick, rightClick);
                            }
                        }
                    }
                    else
                    {
                        // DebugMessage = "Player " + playerID + " no longer tracked!";
                        SetCursor(waitCursor);
                        playerID = -1;
                    }
                }
            }

            if (playerLost)
            {
                // DebugMessage = "Player " + playerID + " lost!";
                SetCursor(waitCursor);
                playerID = -1;
            }
        }
    }
}
